/**
  * The Genetic Algorithm component definition.
  */

#include "printf.h"
#include "bitread.h"

module GeneticAlgorithmC {
	uses {
		interface Random;
	}
	provides {
		interface GeneticAlgorithm;
	}
}
// Maximum population size
#define        GPOPSIZE 100

implementation {

	// Member variables
	uint16_t t0_fitness[GPOPSIZE];
	uint16_t top_fitness_val;
	uint16_t sum_fitness_val;
	uint16_t t1_a_population[GPOPSIZE];
	uint16_t t1_b_population[GPOPSIZE];
	uint16_t bestcandidate_a;
	uint16_t bestcandidate_b;
	uint16_t t0_a_population[GPOPSIZE];
	uint16_t t0_b_population[GPOPSIZE];
	uint16_t next_gen_expected_count[GPOPSIZE];

	uint16_t  bitmutation;
	uint8_t   popsize;
	uint16_t  ngenerations;

	//randomize all genes of each individual in the t0_population
	void randomize_t0_population() {
		uint16_t i;
		uint16_t randNumberA;
		for (i=0 ; i < popsize ; i++) {
		  //randNumberA = random(); 
		  randNumberA = call Random.rand16();
		  t0_a_population[i]=randNumberA;
		}
		for (i=0 ; i < popsize ; i++) {
		  //randNumberA = random(); 
		  randNumberA = call Random.rand16();
		  t0_b_population[i]=randNumberA;
		}
	}

	//store just the current generation fitness to save memory
	void evaluate_t0_fitness() {
		int i;
		top_fitness_val=0;
		sum_fitness_val=0;

		for (i=0 ; i < popsize ; i++) {
			sum_fitness_val = t0_fitness[i]+sum_fitness_val;
		
			if (t0_fitness[i]>top_fitness_val) {
				top_fitness_val=t0_fitness[i];	
				//begin: store the best candidate for viewing
				bestcandidate_a=t0_a_population[i];
				bestcandidate_b=t0_b_population[i];
				//end: store the best candidate for viewing
			}
		}
	}

	void expected_count_t1() {   
		//sort all elements in ascending order
		//based on the fitness stored in t0_fitness[i]...
		uint8_t x;
		uint8_t y;
		uint8_t i;
		uint16_t t;
		uint16_t A;
		uint16_t B;
		uint16_t current_value;
	
		//begin: bubble sort the array
		for (x=0; x < popsize-1; x++) {
			for (y=0; y < popsize-x-1; y++) {
      	if (t0_fitness[y] > t0_fitness[y+1]) {
          	t=t0_fitness[y];
						A=t0_a_population[y];
						B=t0_b_population[y];

          	t0_fitness[y]=t0_fitness[y+1];
          	t0_fitness[y+1]=t;

          	t0_a_population[y]=t0_a_population[y+1];
          	t0_a_population[y+1]=A;

          	t0_b_population[y]=t0_b_population[y+1];
          	t0_b_population[y+1]=B;
      	}
			}
		}
		//end: bubble sort the array		 
	
		//since we limited each fitness element not to surpass 100
		//by letting 100 population size we can have a final sum of 10k.
		//however the unsigned integer can hold at least the number 65,535.
		//this means we could theoretically have a population size of 650. 

	  //next_gen_expected_count[] will have a
	  //cdf on the  next_gen_expected_count[] array
	 	current_value=0;
	  for (i=0 ; i < popsize ; i++) {
	   	current_value=current_value+t0_fitness[i];     
	   	next_gen_expected_count[i]=current_value;    
	  }
	}

	void populate_t1()
	{
		uint8_t i;
		uint8_t j;
		uint16_t randomValue;
		uint8_t select_index;

		for (i=0 ; i < popsize ; i++) {
			//randomValue=random(0,next_gen_expected_count[popsize-1]);
			randomValue = call Random.rand16() % (next_gen_expected_count[popsize-1]+1);
			select_index=0;

			for (j=1 ; j < popsize ; j++) {
				if ((randomValue>=next_gen_expected_count[j-1]) && (randomValue<next_gen_expected_count[j])) {
					select_index=j; 
					j=popsize; //exit the loop
				}  
			}
			t1_a_population[i]= t0_a_population[select_index];
			t1_b_population[i]= t0_b_population[select_index];
		}
	}

	//switch bits between two individuals at a particular cross point
	void mate_t1() {
		uint8_t  i;
		uint8_t  j;
		uint8_t  crossoverSite;
		uint8_t  crossover_siteA;
		uint8_t  crossover_siteB;
		uint16_t randomMutationA;
		uint16_t randomMutationB;
		bool     siteA;
		bool     siteB;

		for (i=0 ; i < popsize ; i=i+2) {
		  //crossoverSite=random(0,31);
		  crossoverSite = call Random.rand16() % 32;

			//begin: mutate genes a bit. For simplicity we just randomize an entire
			//set of genes...
			//randomMutationA=random(1,1000);
			//randomMutationB=random(1,1000);
			randomMutationA=call Random.rand16() %1001 + 1;
			randomMutationB=call Random.rand16() %1001 + 1;
			if (randomMutationA<=bitmutation){ t1_b_population[i] = random(); }
			if (randomMutationB<=bitmutation){ t1_b_population[i] = random(); }
			//end: mutate genes a bit

			if (crossoverSite<16) {
				crossover_siteA=crossoverSite; //keep values on array A till crossoversite
				crossover_siteB=0; //cross all values on array B
			}
			else {
				crossover_siteA=16; //dont mate any values of array A	
				crossover_siteB=32-crossoverSite;
			}

			//mate array A
			for (j=crossover_siteA ; j < 16 ; j++) {
				siteA=bitRead(t1_a_population[i],j);
				siteB=bitRead(t1_a_population[i+1],j);
				bitWrite(t1_a_population[i],j,siteB);
				bitWrite(t1_a_population[i+1],j,siteA);	
			}

			//mate array B
			for (j=crossover_siteB ; j < 16 ; j++)
			{
				siteA=bitRead(t1_b_population[i],j);
				siteB=bitRead(t1_b_population[i+1],j);
				bitWrite(t1_b_population[i],j,siteB);
				bitWrite(t1_b_population[i+1],j,siteA);	
			}
		} //for (int i=0 ; i < popsize ; i=i+2)
	}
	//Function definitions
	
	/**
	  * initialize
	  * Takes the place of the GA::GA constructor in C++
	  */
	command void GeneticAlgorithm.initialize_engine (uint8_t _popsize, uint16_t _ngenerations, uint16_t _bitmutation) {
		popsize      = _popsize;
		ngenerations = _ngenerations;
		bitmutation  = _bitmutation;
	
		//initialize the random population
		randomize_t0_population();
	}

	//processes all the steps for current generation
	command void GeneticAlgorithm.process_generation(uint16_t generationid)
	{
		//calculates the average and find the best individual
		evaluate_t0_fitness();

		//whats the probability of a particular individual
		//show up on t1?
		expected_count_t1();

		//populate the next generation array with the best performing individuals
		populate_t1();

		//print some pre-mate statistics
		//report_pre_mate_statistics(generationid);
	
		//mate every two elements at a random crossover point
		mate_t1();
	}

	//in essence, just copies each element of t1 into t0
	command void GeneticAlgorithm.prepare_next_generation() {
		uint8_t i;
		for (i=0 ; i < popsize ; i++) {
		  t0_a_population[i]=t1_a_population[i];
		  t0_b_population[i]=t1_b_population[i];
		} 
	}
	
	// enters a computed fitness value into the array
	command	void GeneticAlgorithm.write_t0_fitness(uint8_t idx, uint16_t value) {
		t0_fitness[idx]=value;
	}

	// reads population member idx from the a population
	command	uint16_t GeneticAlgorithm.read_t0_a_population (uint8_t idx) {
		return (t0_a_population[idx]);
	}

	// reads population member idx from the b population
	command	uint16_t GeneticAlgorithm.read_t0_b_population (uint8_t idx) {
		return (t0_b_population[idx]);
	}

	// displays some statistics about the curent state of the genetic algorithm
	// must be called multiple times 
	command void GeneticAlgorithm.report_statistics(uint16_t generationid, uint8_t reportstep ) {
		//uint8_t i;
		
		switch (reportstep) {
			case 0:
				printf("generation=%u, ",generationid);
				break;
			case 1:
				printf("top fitness=%u, ",top_fitness_val);
				break;
			case 2:
				printf("sum fitness=%u\n",sum_fitness_val);
				break;
			case 3:
				printf("top candidate (BIN): %x ", bestcandidate_a);
				break;
			case 4:
				printf("%x\n", bestcandidate_b);
				break;
			case 5:
				printfflush();
				break;
		}
/*	
		if (verbosityLevel>=2) {
			printf("printing each individual+fitness\n");
			for (i=0 ; i < popsize ; i++)	{
				printf ("%x %x (%u)\n",t0_a_population[i],t0_b_population[i],t0_fitness[i]);
				printfflush();
			}

			printf("printing next gen expected count\n");
			for (i=0 ; i < popsize ; i++)	{
				printf ("individual= %u, %u\n",i,next_gen_expected_count[i]);
				printfflush();
			}
		
			printf("printing each of next gen individuals\n");
			for (i=0 ; i < popsize ; i++) {
				printf ("%x %x\n",t1_a_population[i],t1_b_population[i]);
				printfflush();
			}
		}
*/
	}
}

